/*******************************************************************
 *
 * Copyright (C), 2002-2020, www.lifesense.com, All rights reserved.
 *
 * Author      : xiaoyu | xiaoyu_ebox@163.com
 * Create Time : 2020-10-4
 * Version     : v0.1
 * Description : 
 *******************************************************************/
/*----------------------------Head file----------------------------*/
#include "mmal.h"
#include "core/mmal_component_private.h"
#include "core/mmal_port_private.h"
#include "mmal_logging.h"
#include "stdio.h"
#include <time.h>
#include <unistd.h>


#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <fcntl.h>


#include <sys/socket.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <errno.h>
#include <linux/videodev2.h>

#include <sys/shm.h>
#include <unistd.h>
#include <execinfo.h>
#include <linux/fb.h>
#include <stdbool.h>

/*--------------------------config define--------------------------*/
// 默认不保存到文件
#define SAVE_TO_FILE            0

#define USB_CAM_PORTS_NUM       3

/*----------------------------macro file---------------------------*/
#define LogFunEntry()           printf(">>[%s:%u]\n", __FUNCTION__, __LINE__)
#define LogNote(_fmt, ...)      printf("[%s:%u]" _fmt, __FUNCTION__, __LINE__, ##__VA_ARGS__)

/*----------------------------type define--------------------------*/
typedef struct VideoBuffer {
    void   *start;
    size_t  length;
} VideoBuffer;

/*-------------------------func declarative------------------------*/
static bool FfmpegDecoderInit(void);

/*--------------------------var declarative------------------------*/ 
int sg_CamFd = -1;
VideoBuffer *sg_CamBuffers = NULL;

MMAL_QUEUE_T *sg_bufQueue = NULL;
MMAL_BOOL_T sg_bQuit = MMAL_FALSE;
MMAL_BOOL_T sg_enable = MMAL_FALSE;

#if SAVE_TO_FILE
int s32SaveFd = -1;
#endif

/*-----------------------------------------------------------------*/

static MMAL_STATUS_T usb_cam_component_disable(MMAL_COMPONENT_T *component)
{
    LogFunEntry();

    return MMAL_SUCCESS;
}

static MMAL_STATUS_T usb_cam_component_enable(MMAL_COMPONENT_T *component)
{
    LogFunEntry();

    return MMAL_SUCCESS;
}

/** Destroy a previously created component */
static MMAL_STATUS_T usb_cam_component_destroy(MMAL_COMPONENT_T *component)
{
    LogFunEntry();

#if SAVE_TO_FILE
    if(s32SaveFd >= 0)
    {
        close(s32SaveFd);
        s32SaveFd = -1;
    }
#endif
    
    if(component->output_num)
        mmal_ports_free(component->output, component->output_num);
    return MMAL_SUCCESS;
}

/** Get port context data. */
static MMAL_STATUS_T usb_cam_port_info_get(MMAL_PORT_T *port)
{
    LogNote("Port: %u\n", port->index_all);

    if(port->index_all == 0)
    {
        port->buffer_num_min = 4;
        port->buffer_num = 4;
        port->buffer_size_min = 740;
        port->buffer_size = 740;

        port->format->type = MMAL_ES_TYPE_CONTROL;
    }
    else if(port->index_all == 1)
    {
        port->buffer_num_min = 1;
        port->buffer_num = 1;
        port->buffer_size_min = 7680;
        port->buffer_size_recommended = 7680;
        port->buffer_size = 7680;
        port->capabilities = MMAL_PORT_CAPABILITY_ALLOCATION;

        port->format->type = MMAL_ES_TYPE_VIDEO;
        port->format->encoding = MMAL_ENCODING_YUYV;
        port->format->es->video.width = 320;
        port->format->es->video.height= 240;
        port->format->es->video.frame_rate.num = 0;
        port->format->es->video.frame_rate.den = 65536;
    }
    else if(port->index_all == 2)
    {
        port->buffer_num_min = 1;
        port->buffer_num_recommended = 1;
        port->buffer_num = 1;
        port->buffer_size_min = 15360;
        port->buffer_size_recommended = 15360;
        port->buffer_size = 15360;
        port->capabilities = MMAL_PORT_CAPABILITY_ALLOCATION;

        port->format->type = MMAL_ES_TYPE_VIDEO;
        port->format->encoding = MMAL_ENCODING_YUYV;
        port->format->es->video.width = 640;
        port->format->es->video.height= 480;
        port->format->es->video.frame_rate.num = 0;
        port->format->es->video.frame_rate.den = 65536;
    }
    else if(port->index_all == 3)
    {
        port->buffer_num_min = 1;
        port->buffer_num_recommended = 1;
        port->buffer_num = 1;
        port->buffer_size_min = 15360;
        port->buffer_size_recommended = 15360;
        port->buffer_size = 15360;
        port->capabilities = MMAL_PORT_CAPABILITY_ALLOCATION;

        port->format->type = MMAL_ES_TYPE_VIDEO;
        port->format->encoding = MMAL_ENCODING_YUYV;
        port->format->es->video.width = 640;
        port->format->es->video.height= 480;
        port->format->es->video.frame_rate.num = 0;
        port->format->es->video.frame_rate.den = 0;
    }

    LOG_TRACE("Buf: %u,%u,%u, %u,%u,%u, %u, %u, %u\n", port->buffer_num_min,
                                        port->buffer_num_recommended,
                                        port->buffer_num,
                                        port->buffer_size_min,
                                        port->buffer_size_recommended,
                                        port->buffer_size,
                                        port->buffer_alignment_min,
                                        port->is_enabled,
                                        port->capabilities);
    LOG_TRACE("Format: %u, %c%c%c%c, %c%c%c%c, %u, %u, %u\n", port->format->type,
                                      (port->format->encoding)&0xFF,
                                      (port->format->encoding>>8)&0xFF,
                                      (port->format->encoding>>16)&0xFF,
                                      (port->format->encoding>>24)&0xFF,
                                      (port->format->encoding_variant)&0xFF,
                                      (port->format->encoding_variant>>8)&0xFF,
                                      (port->format->encoding_variant>>16)&0xFF,
                                      (port->format->encoding_variant>>24)&0xFF,
                                      port->format->bitrate,
                                      port->format->flags,
                                      port->format->extradata_size);
    LOG_TRACE("Format V: %u, %u, %u,%u, %u,%u, %u\n", port->format->es->video.width,
                                      port->format->es->video.height,
                                      port->format->es->video.frame_rate.num,
                                      port->format->es->video.frame_rate.den,
                                      port->format->es->video.par.num,
                                      port->format->es->video.par.den,
                                      port->format->es->video.color_space);
    return MMAL_SUCCESS;
}

/** Set port context data. */
static MMAL_STATUS_T usb_cam_port_info_set(MMAL_PORT_T *port)
{
    LogNote("Port: %u\n", port->index_all);
    
    LOG_TRACE("Buf: %u,%u,%u, %u,%u,%u, %u, %u, %u\n", port->buffer_num_min,
                                        port->buffer_num_recommended,
                                        port->buffer_num,
                                        port->buffer_size_min,
                                        port->buffer_size_recommended,
                                        port->buffer_size,
                                        port->buffer_alignment_min,
                                        port->is_enabled,
                                        port->capabilities);
   LOG_TRACE("Format: %u, %c%c%c%c, %c%c%c%c, %u, %u, %u\n", port->format->type,
                                      (port->format->encoding)&0xFF,
                                      (port->format->encoding>>8)&0xFF,
                                      (port->format->encoding>>16)&0xFF,
                                      (port->format->encoding>>24)&0xFF,
                                      (port->format->encoding_variant)&0xFF,
                                      (port->format->encoding_variant>>8)&0xFF,
                                      (port->format->encoding_variant>>16)&0xFF,
                                      (port->format->encoding_variant>>24)&0xFF,
                                      port->format->bitrate,
                                      port->format->flags,
                                      port->format->extradata_size);
    LOG_TRACE("Format V: %u, %u, %u,%u, %u,%u, %u\n", port->format->es->video.width,
                                      port->format->es->video.height,
                                      port->format->es->video.frame_rate.num,
                                      port->format->es->video.frame_rate.den,
                                      port->format->es->video.par.num,
                                      port->format->es->video.par.den,
                                      port->format->es->video.color_space);
    return MMAL_SUCCESS;
}


/** Enable processing on a port */
static MMAL_STATUS_T usb_cam_port_enable(MMAL_PORT_T *port, MMAL_PORT_BH_CB_T cb)
{
    LogNote("Port: %u\n", port->index_all);

    MMAL_PARAM_UNUSED(cb);

    if(port->index_all == 1)
    {
        // 请求分配内存
    	struct v4l2_requestbuffers req;
    	req.count  = 2;
    	req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    	req.memory = V4L2_MEMORY_MMAP;
    	if (ioctl(sg_CamFd, VIDIOC_REQBUFS, &req) == -1) {
    		return -1;
    	}

    	// 获取内存地址
    	int numBufs;
    	struct v4l2_buffer  camFrame;
    	sg_CamBuffers = calloc(req.count, sizeof(VideoBuffer));
    	for(numBufs = 0; numBufs < req.count; numBufs++) {
    		memset( &camFrame, 0, sizeof(camFrame) );
    		camFrame.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    		camFrame.memory = V4L2_MEMORY_MMAP;
    		camFrame.index = numBufs;
    		// 读取缓存
    		if (ioctl(sg_CamFd, VIDIOC_QUERYBUF, &camFrame) == -1) {
    			return -1;
    		}

    		sg_CamBuffers[numBufs].length = camFrame.length;
    		// 转换成相对地址
    		sg_CamBuffers[numBufs].start = mmap(NULL, camFrame.length, PROT_READ | PROT_WRITE, MAP_SHARED, sg_CamFd, camFrame.m.offset);

    		if (sg_CamBuffers[numBufs].start == MAP_FAILED) {
    			return -1;
    		}

    		// 放入缓存队列
    		if (ioctl(sg_CamFd, VIDIOC_QBUF, &camFrame) == -1) {
    			return -1;
    		}
    	}


        // 开始录制
    	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        int status = ioctl(sg_CamFd, VIDIOC_STREAMON, &type);
        if (status < 0) {
            printf("VIDIOC_STREAMON failed (%d)\n", status);
        }
        else
        {
            sg_enable = true;
        }
    }
    
    return MMAL_SUCCESS;
}

/** Disable processing on a port */
static MMAL_STATUS_T usb_cam_port_disable(MMAL_PORT_T *port)
{
    LogNote("Port: %u\n", port->index_all);

    sg_enable = MMAL_FALSE;
    sg_bQuit = MMAL_TRUE;

    MMAL_STATUS_T status = mmal_component_action_deregister(port->component);
    LogNote("action_deregister: %u\n", status);

    close(sg_CamFd);
    sg_CamFd = -1;

    // Release the resource
   int numBufs;
    for(numBufs = 0; numBufs < 2; numBufs++) {
        munmap(sg_CamBuffers[numBufs].start, sg_CamBuffers[numBufs].length);
    }

   while (mmal_queue_length(sg_bufQueue) > 0)
   {
        MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(sg_bufQueue);
        mmal_port_buffer_header_callback(port, buffer);
   }
    
    

    return MMAL_SUCCESS;
}

/** Flush a port */
static MMAL_STATUS_T usb_cam_port_flush(MMAL_PORT_T *port)
{
    LogNote("Port: %u\n", port->index_all);

    return MMAL_SUCCESS;
}

/** Send a buffer header to a port */
static MMAL_STATUS_T usb_cam_port_send(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
//    LogNote("Port: %u\n", port->index_all);

    /* Handle event buffers */
   if (buffer->cmd)
   {
      buffer->length = 0;
      mmal_port_buffer_header_callback(port, buffer);
      return MMAL_SUCCESS;
   }

    if(sg_bufQueue != NULL)
    {
        mmal_queue_put(sg_bufQueue, buffer);
        mmal_component_action_trigger(port->component);
    }

    return MMAL_SUCCESS;
}

bool bMjpegFormat = false;
/** Set format on a port */
static MMAL_STATUS_T usb_cam_port_format_commit(MMAL_PORT_T *port)
{
    LogNote("Port: %u\n", port->index_all);

    LogNote("Set fd:%d W:%u, H:%u, FR:%u/%u, Format:%c%c%c%c\n", sg_CamFd, port->format->es->video.width,
                                           port->format->es->video.height,
                                           port->format->es->video.frame_rate.num,
                                           port->format->es->video.frame_rate.den,
                                           (port->format->encoding)&0xFF,
                                           (port->format->encoding>>8)&0xFF,
                                           (port->format->encoding>>16)&0xFF,
                                           (port->format->encoding>>24)&0xFF);

    // 获取当前的视频格式
    int ret;
	struct v4l2_format fmt;
	unsigned char *pixformat = (unsigned char *)&fmt.fmt.pix.pixelformat;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = ioctl(sg_CamFd, VIDIOC_G_FMT, &fmt);
	
	fmt.fmt.pix.width  = port->format->es->video.width;
	fmt.fmt.pix.height = port->format->es->video.height;
	if(port->format->encoding == MMAL_ENCODING_YUYV)
	{
        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    }
    else if(port->format->encoding == MMAL_ENCODING_MJPEG)
	{
        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    }
	ret = ioctl(sg_CamFd, VIDIOC_S_FMT, &fmt);
	if (ret < 0) {
		printf("\nSet format failed\n");
		return MMAL_EIO;
	}

	// 设置帧率
	if((port->format->es->video.frame_rate.num == 0)
	    || (port->format->es->video.frame_rate.den/port->format->es->video.frame_rate.num) < 10)
	{   // 最低10帧
        port->format->es->video.frame_rate.num = 10;
        port->format->es->video.frame_rate.den = 1;
	}
	struct v4l2_streamparm setfps;
	memset(&setfps, 0, sizeof(setfps));
	setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = ioctl(sg_CamFd, VIDIOC_G_PARM, &setfps);
	
    setfps.parm.capture.timeperframe.numerator = port->format->es->video.frame_rate.num;
    setfps.parm.capture.timeperframe.denominator = port->format->es->video.frame_rate.den;
	ret = ioctl(sg_CamFd, VIDIOC_S_PARM, &setfps);
	if(ret < 0) {
		perror("\nUnable to set framerate");
		return MMAL_EIO;
	}

	// 读取参数
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl(sg_CamFd, VIDIOC_G_FMT, &fmt);
	printf("==>Curr format:\n\tFormat    : %c%c%c%c\n" \
									"\tWidth     : %d\n" \
									"\tHeight    : %d\n",	pixformat[0], pixformat[1], pixformat[2], pixformat[3],
														fmt.fmt.pix.width,
														fmt.fmt.pix.height);
    if(pixformat[0] == 'M')
    {
        bMjpegFormat = true;
        FfmpegDecoderInit();
    }

	// 读取帧率
	memset(&setfps, 0, sizeof(setfps));
	setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
	ret = ioctl(sg_CamFd, VIDIOC_G_PARM, &setfps);
	if(ret < 0) {
		perror("Unable to get framerate");
		return MMAL_EIO;
	}
	
	printf("\tFrame rate: %u/%u\n", setfps.parm.capture.timeperframe.denominator,
									 setfps.parm.capture.timeperframe.numerator);

#if SAVE_TO_FILE
    if(s32SaveFd < 0)
    {
        const char *pcFileName = "./cam_yuyv.raw";
        if(port->format->encoding == MMAL_ENCODING_MJPEG)
        {
            pcFileName = "./cam_mjpeg.raw";
        }
        s32SaveFd = open(pcFileName, O_WRONLY|O_TRUNC|O_CREAT, 0666);
    }
#endif

    return MMAL_SUCCESS;
}

/** Connect 2 ports together */
static MMAL_STATUS_T usb_cam_port_connect(MMAL_PORT_T *port, MMAL_PORT_T *other_port)
{
    LogNote("Port: %u\n", port->index_all);
    
    MMAL_PARAM_UNUSED(other_port);
    return MMAL_SUCCESS;
}

/** Set parameter on a port */
static MMAL_STATUS_T usb_cam_port_parameter_set(MMAL_PORT_T *port, const MMAL_PARAMETER_HEADER_T *param)
{
    LogNote("Port: %u  id:%08x, size:%u\n", port->index_all, param->id, param->size);

    usb_cam_port_info_set(port);
    
    return MMAL_SUCCESS;
}

/** Get parameter on a port */
static MMAL_STATUS_T usb_cam_port_parameter_get(MMAL_PORT_T *port, MMAL_PARAMETER_HEADER_T *param)
{
    LogNote("Port: %u  id:%08x, size:%u\n", port->index_all, param->id, param->size);

    usb_cam_port_info_get(port);

    return MMAL_SUCCESS;
}



int preinit_camera(uint32_t camNum)
{
    int ret;
	struct v4l2_format fmt;

	char devName[sizeof("/dev/video") + 3] = {0};
	sprintf(devName, "/dev/video%u", camNum);

	printf("Open Cam:%s\n", devName);

	int try_count = 0;
	sg_CamFd = open(devName, O_RDWR | O_NONBLOCK, 0);
	while((sg_CamFd < 0) && (try_count++ < 10)) {
		sleep(1);
		sg_CamFd = open(devName, O_RDWR | O_NONBLOCK, 0);
	};
	if(try_count >= 10) {
		printf("Cannot open device:%d\n", sg_CamFd);
		return -1;
	}


	// 获取驱动信息
	struct v4l2_capability cap;
	
	ret = ioctl(sg_CamFd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		perror("ioctl querycap");
		return -1;
	}
	
	printf("==>Cap info:\n");
	printf("\tCard     : %s\n", cap.driver);
	printf("\tDriver   : %s\n", cap.driver);
	printf("\tVersion  : %u.%u.%u\n", (cap.version>>16)&0xFF, (cap.version>>8)&0xFF, cap.version&0xFF);
	printf("\tCap      : %04x\n", cap.capabilities);
	printf("\tBus info : %s\n", cap.bus_info);

	if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
		printf("video device donot support capture\n");
		return -1;
	}
	
	// 获取支持的格式、分辨率
	struct v4l2_fmtdesc fmtdesc;
	struct v4l2_frmsizeenum frmsize;
	
	fmtdesc.index = 0;
	fmtdesc.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	
	printf("==>Support format:\n");
	while(ioctl(sg_CamFd, VIDIOC_ENUM_FMT, &fmtdesc) >= 0) {
		printf("\t%d: %s\n", fmtdesc.index+1, fmtdesc.description);
		
		frmsize.index = 0;
		frmsize.pixel_format = fmtdesc.pixelformat;
		while (ioctl(sg_CamFd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0) {
			if(frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
				printf("\t\t%dx%d\n", frmsize.discrete.width, frmsize.discrete.height);
			}else if(frmsize.type == V4L2_FRMSIZE_TYPE_STEPWISE
					|| frmsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) {
				printf("\t\t{%u-%u, %u}x{%u-%u, %u}\n",
					frmsize.stepwise.min_width,
					frmsize.stepwise.max_width,
					frmsize.stepwise.step_width,
					frmsize.stepwise.min_height,
					frmsize.stepwise.max_height,
					frmsize.stepwise.step_height);
			}
			frmsize.index++;
		}

		fmtdesc.index++;
	}


	// 获取当前的视频格式
	unsigned char *pixformat = (unsigned char *)&fmt.fmt.pix.pixelformat;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl(sg_CamFd, VIDIOC_G_FMT, &fmt);
	printf("==>Currentdata format:\n\tFormat    : %c%c%c%c\n" \
									"\tWidth     : %d\n" \
									"\tHeight    : %d\n",	pixformat[0], pixformat[1], pixformat[2], pixformat[3],
														fmt.fmt.pix.width,
														fmt.fmt.pix.height);

	// 设置帧率
	struct v4l2_streamparm setfps;
	memset(&setfps, 0, sizeof(setfps));
	setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
	ret = ioctl(sg_CamFd, VIDIOC_G_PARM, &setfps);
	if(ret < 0) {
		perror("Unable to get framerate");
		return -1;
	}
	
	printf("\tFrame rate: %u/%u\n", setfps.parm.capture.timeperframe.denominator,
									 setfps.parm.capture.timeperframe.numerator);

	printf("Preinit success.\n");

    return 0;
}

#include <libavformat/avformat.h>
#include <libavutil/avutil.h>

AVFrame *pAVFrame = NULL;
AVCodecContext *pAVCodecContext = NULL;
AVCodec *pMjpegDecoder = NULL;
bool bInit = false;

bool FfmpegDecoderInit(void)
{
    if(bInit)
        return true;

    // 1. 注册所有编解码器
    av_register_all();

    // 2. 创建并配置codecContext
#if 0
    pMjpegDecoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
    pAVCodecContext = avcodec_alloc_context3(pMjpegDecoder);
    avcodec_get_context_defaults3(pAVCodecContext, pMjpegDecoder);
    avcodec_open2(pAVCodecContext, pMjpegDecoder, NULL);
#else
    
    pMjpegDecoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
    if(pMjpegDecoder == NULL)
    {
        return false;
    }    

    pAVCodecContext = avcodec_alloc_context3(pMjpegDecoder);
    
    //初始化参数，下面的参数应该由具体的业务决定  AV_PIX_FMT_YUV420P;
    pAVCodecContext->codec_type = AVMEDIA_TYPE_VIDEO;
    pAVCodecContext->width = 1280;//视频宽
    pAVCodecContext->height = 720;//视频高
    pAVCodecContext->pix_fmt = AV_PIX_FMT_YUV422P;
    pAVCodecContext->color_range = AVCOL_RANGE_MPEG;

    avcodec_open2(pAVCodecContext, pMjpegDecoder, NULL);
#endif
    bInit = true;

    printf("Ffmpeg init ok.\n");
    return true;
}

uint32_t MjpegToYuv(uint8_t *data, uint32_t dataSize, uint8_t *outBuffer, uint32_t outBufSize)
{   
    // 1. 将元数据装填到packet
    uint32_t u32DecSize = 0;
    AVPacket *avPkt = av_packet_alloc();
    avPkt->size = dataSize;
    avPkt->data = data;

    // 2. 解码
    int ret = avcodec_send_packet(pAVCodecContext, avPkt);
    if (ret >=0) {
        AVFrame *pYUVFrame = av_frame_alloc();
        ret = avcodec_receive_frame(pAVCodecContext, pYUVFrame);
        if (ret >= 0) {
            if((pYUVFrame->format == AV_PIX_FMT_YUVJ422P)
                || (pYUVFrame->format == AV_PIX_FMT_YUV422P)
                || (pYUVFrame->format == AV_PIX_FMT_YUYV422))
            {
                u32DecSize = (pYUVFrame->linesize[0] * pYUVFrame->height) << 1;
            }
            else if((pYUVFrame->format == AV_PIX_FMT_YUVJ420P)
                || (pYUVFrame->format == AV_PIX_FMT_YUV420P))
            {
                u32DecSize = (pYUVFrame->linesize[0] * pYUVFrame->height * 3) >> 1;
            }
            
//            printf("Decode %u*%u(%u) nb:%u Q:%u Size:%u-%u-%u-%u  %u-%u\n",
//                pYUVFrame->width,
//                pYUVFrame->height,
//                pYUVFrame->format,
//                pYUVFrame->nb_samples,
//                pYUVFrame->quality,
//                pYUVFrame->linesize[0],
//                pYUVFrame->linesize[1],
//                pYUVFrame->linesize[2],
//                pYUVFrame->linesize[3],
//                outBufSize,
//                u32DecSize);

            if(outBufSize >= u32DecSize)
            {
                memcpy(outBuffer, pYUVFrame->data[0], pYUVFrame->linesize[0]*pYUVFrame->height);
                outBuffer += pYUVFrame->linesize[0]*pYUVFrame->height;
                memcpy(outBuffer, pYUVFrame->data[1], pYUVFrame->linesize[1]*pYUVFrame->height);
                outBuffer += pYUVFrame->linesize[1]*pYUVFrame->height;
                memcpy(outBuffer, pYUVFrame->data[2], pYUVFrame->linesize[2]*pYUVFrame->height);
            }
        }
        else
        {
            printf("-----1\n");
        }
        // free context and avFrame
        av_frame_free(&pYUVFrame);
    }
    else
    {
        printf("-----2\n");
    }
    
    av_packet_unref(avPkt);
    av_packet_free(&avPkt);

    return u32DecSize;
}

/*****************************************************************************/
static void usb_cam_do_processing(MMAL_COMPONENT_T *component)
{
    MMAL_BUFFER_HEADER_T *buffer;
    MMAL_STATUS_T status;

    fd_set fds;
    struct timeval tv;

    struct v4l2_buffer  camFrame2;
    memset( &camFrame2, 0, sizeof(camFrame2) );
	camFrame2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	camFrame2.memory = V4L2_MEMORY_MMAP;


    if(sg_bQuit == MMAL_TRUE)
    {
        return;
    }
    if(sg_enable == MMAL_FALSE)
    {
        return;
    }
	
    do {
        FD_ZERO(&fds);
		FD_SET(sg_CamFd, &fds);

		/* Timeout. */
		tv.tv_sec = 8;
		tv.tv_usec = 0;

		status = select(sg_CamFd + 1, &fds, NULL, NULL, &tv);

		// Get frame
		status = ioctl(sg_CamFd, VIDIOC_DQBUF, &camFrame2);
		if (status < 0) {
			printf("VIDIOC_DQBUF1 failed (%d)\n", status);
			break;
		}
//		printf("----->%u %p %u\n", camFrame2.index, sg_CamBuffers[camFrame2.index].start, camFrame2.bytesused);

#if SAVE_TO_FILE
        if((s32SaveFd > 0) && (camFrame2.bytesused > 0))
        {
            write(s32SaveFd, sg_CamBuffers[camFrame2.index].start, camFrame2.bytesused);
        }
#endif

        /* Get a buffer from this port */
        buffer = mmal_queue_get(sg_bufQueue);
        if(buffer != NULL)
        {
            mmal_buffer_header_mem_lock(buffer);

            if(bMjpegFormat)
            {
                buffer->length = MjpegToYuv(sg_CamBuffers[camFrame2.index].start, camFrame2.bytesused, buffer->data, buffer->alloc_size);
            }
            else
            {
                if(buffer->alloc_size >= camFrame2.bytesused)
                {
                    memcpy(buffer->data, sg_CamBuffers[camFrame2.index].start, camFrame2.bytesused);
                    buffer->length = camFrame2.bytesused;
                }
            }
            
            mmal_buffer_header_mem_unlock(buffer);

            /* Send buffer back */
            mmal_port_buffer_header_callback(component->output[0], buffer);
        }

        // Re-queen buffer
		status = ioctl(sg_CamFd, VIDIOC_QBUF, &camFrame2);
		if (status < 0) {
			printf("VIDIOC_QBUF2 failed (%d)\n", status);
			break;
		}
    }while(0);
}

/** Create an instance of a component  */
static MMAL_STATUS_T mmal_component_create_usb_cam(const char *name, MMAL_COMPONENT_T *component)
{
    LogFunEntry();
    
    MMAL_STATUS_T status = MMAL_ENOMEM;
    unsigned int i;

    uint32_t u32CamNum = 0;
    const char *dot = strchr(name, '.');
    if((dot != NULL) && (dot[1] != '\0'))
    {
        u32CamNum = atoi(dot+1);
    }
    if(preinit_camera(u32CamNum) != 0)
    {
        return MMAL_EAGAIN;
    }

    /* populate component structure */
    component->priv->pf_enable = usb_cam_component_enable;
    component->priv->pf_disable = usb_cam_component_disable;
    component->priv->pf_destroy = usb_cam_component_destroy;

    /* Allocate our local ports. Control port reallocated to set module size. */
    mmal_port_free(component->control);
    component->control = mmal_port_alloc(component, MMAL_PORT_TYPE_CONTROL, 0);
    if (!component->control)
        goto error;

    /* Allocate all the ports for this component */
    component->output = mmal_ports_alloc(component, USB_CAM_PORTS_NUM, MMAL_PORT_TYPE_OUTPUT, 0);
    if(!component->output)
      goto error;
    component->output_num = USB_CAM_PORTS_NUM;

    // 所有port中的序号
    int usb_cam_index = 0;

    component->control->component = component;
    component->control->priv->pf_enable = usb_cam_port_enable;
    component->control->priv->pf_disable = usb_cam_port_disable;
    component->control->priv->pf_flush = usb_cam_port_flush;
    component->control->priv->pf_send = usb_cam_port_send;
    component->control->priv->pf_set_format = usb_cam_port_format_commit;
    component->control->priv->pf_connect = usb_cam_port_connect;
    component->control->priv->pf_parameter_set = usb_cam_port_parameter_set;
    component->control->priv->pf_parameter_get = usb_cam_port_parameter_get;
    component->control->index = 0;
    component->control->index_all = usb_cam_index++;
    usb_cam_port_info_get(component->control);

    
    for(i = 0; i < component->output_num; i++)
    {
        component->output[i]->component = component;
        component->output[i]->priv->pf_enable = usb_cam_port_enable;
        component->output[i]->priv->pf_disable = usb_cam_port_disable;
        component->output[i]->priv->pf_flush = usb_cam_port_flush;
        component->output[i]->priv->pf_send = usb_cam_port_send;
        component->output[i]->priv->pf_set_format = usb_cam_port_format_commit;
        component->output[i]->priv->pf_connect = usb_cam_port_connect;
        component->output[i]->priv->pf_parameter_set = usb_cam_port_parameter_set;
        component->output[i]->priv->pf_parameter_get = usb_cam_port_parameter_get;
        component->output[i]->buffer_num_min = 1;
        component->output[i]->buffer_num_recommended = 1;
        component->output[i]->index = i;
        component->output[i]->index_all = usb_cam_index++;

        usb_cam_port_info_get(component->output[i]);
    }


    sg_bufQueue = mmal_queue_create();
    if(sg_bufQueue == NULL)
        goto error;

    status = mmal_component_action_register(component, usb_cam_do_processing);
    if (status != MMAL_SUCCESS)
      goto error;

    mmal_component_action_trigger(component);
    LogNote("usb_cam create success, portCnt:%u.\n", usb_cam_index);

    return MMAL_SUCCESS;

error:
    usb_cam_component_destroy(component);
    return status;
}

MMAL_CONSTRUCTOR(mmal_register_component_usb_cam);
void mmal_register_component_usb_cam(void)
{
    mmal_component_supplier_register("usb_cam", mmal_component_create_usb_cam);
}

